#!/usr/bin/env python
"""
Add your desired parameters here. All required headers will be generated from this.
!!!IMPORTANT this file must be executable !!!

Use one of these commands to add parameters to your parameter struct:

def add(self, name, paramtype, description, level=0, edit_method='""', default=None, min=None, max=None, configurable=False, global_scope=False, constant=False):
        Adds parameters to your parameter struct.

def add_enum(self, name, description, entry_strings, default=None, paramtype='int'):
        Adds an enum to dynamic reconfigure

def add_group(self, name):
        Create a parameter group for the dynamic_reconfigure window

def add_publisher(self, name, message_type, description, default_topic="", default_queue_size=5, topic_param=None,
                  queue_size_param=None, header=None, module=None, configurable=False, global_scope=False,
                  constant=False):
        Adds an initialized publisher to your parameter struct and parameter for its topic and queue size

def add_subscriber(self, name, message_type, description, default_topic="", default_queue_size=5, no_delay=False,
                   topic_param=None, queue_size_param=None, header=None, module=None, configurable=False,
                   global_scope=False, constant=False):
        Adds an initialized subscriber to your parameter struct and parameters for topic and queue size.


For more information on the API, see here: https://gitlab.mrt.uni-karlsruhe.de/MRT/rosinterface_handler/blob/master/doc/HowToWriteYourFirstInterfaceFile.md
"""
from rosinterface_handler.interface_generator_catkin import *

gen = InterfaceGenerator()

# Add a parameter for changing verbosity (=log level) on the fly
gen.add_verbosity_param(configurable=True)

# Diagnostics
gen.add("diag_pub_topic", paramtype='std::string', description="Topicname for diagnostic publisher", default="out_topic_diagnosed")
gen.add("diagnostic_updater_name", paramtype='std::string', description="Name of diagnostic updater.", default="MonoLidarUpdater")
gen.add("diagnostic_updater_hardware_id", paramtype='std::string', description="Identifier for hardware.", default="MonoLidar")
gen.add("diagnostic_updater_rate", paramtype='double', description="Expected updater frequency", default=1)
gen.add("diagnostic_updater_rate_tolerance", paramtype='double', description="Tolerance with which bounds must be satisfied.", default=1)

# Your Params here
gen.add("prior_vehicle_frame", paramtype='std::string', description="frame id of vehicle from prior(used for tf), if empty, internal prior estimation is used.", default="", configurable=True, global_scope=False)
gen.add("prior_global_frame", paramtype='std::string', description="frame id of origin from prior(used for tf)", default="origin", configurable=True, global_scope=False)
gen.add("tf_waiting_time", paramtype='double', description="time in sec that lookup transform from tf wiats before throwing exception", default="10.0", configurable=True, global_scope=False)

# Frame ids
gen.add("calib_target_frame_id", paramtype='std::string', description="target frame id of calibration", default="camera", configurable=True, global_scope=False)
gen.add("calib_source_frame_id", paramtype='std::string', description="source frame id of calibration", default="vehicle", configurable=True, global_scope=False)

gen.add("tf_parent_frame_id", paramtype='std::string', description="parent frame id for tf publishing if empty nothing is published; format: tf2 convention (without / at beginning)", default="local_cs", configurable=True)
gen.add("tf_child_frame_id", paramtype='std::string', description="child frame id for tf publishing if empty nothing is published; format: tf2 convention (without / at beginning)", default="", configurable=True)

# Publisher and subscriber (group name will show up in dynamic_reconfigure window)
pub_sub = gen.add_group("Publisher and subscriber")
pub_sub.add_subscriber("tracklets_subscriber", message_type="matches_msg_depth_ros::MatchesMsgWithOutlierFlag", description="tracklets with depth and outlier flags", default_topic="in_topic_tracklets", no_delay=True, configurable=True)
pub_sub.add_subscriber("camera_info_subscriber", message_type="sensor_msgs::CameraInfo", description="camera info for intrinsics", default_topic="in_topic_cam_info", no_delay=True, configurable=True)
pub_sub.add_publisher("active_path_publisher", message_type="nav_msgs::Path", description="publish path that is actively optimized in bundle adjustment for plotting", default_topic="active_path_topic", configurable=True)
pub_sub.add_publisher("path_publisher", message_type="nav_msgs::Path", description="publish whole path for plotting", default_topic="path_topic", configurable=True)
pub_sub.add_publisher("landmarks_publisher", message_type="sensor_msgs::PointCloud2", description="publish landmarks in global frame", default_topic="landmarks_topic", configurable=True)
pub_sub.add_publisher("planes_publisher", message_type="visualization_msgs::MarkerArray", description="publish estiamted local planes in global frame", default_topic="planes_topic", configurable=True)

# Bundle adjustment parameters
gen.add("max_number_landmarks_near_bin", paramtype='int', description="number of landmarks in near bin by observation delta angle", default="300", min="10", max="5000", configurable=True)
gen.add("max_number_landmarks_middle_bin", paramtype='int', description="number of landmarks in middle bin chosen randomly", default="300", min="10", max="5000", configurable=True)
gen.add("max_number_landmarks_far_bin", paramtype='int', description="number of landmarks in far bin by tracklength", default="300", min="10", max="5000", configurable=True)
gen.add("time_between_keyframes_sec", paramtype='double', description="time in seconds between the keyframes", default="0.1", min="0.01", max="1.0", configurable=True)
gen.add("min_median_flow", paramtype='double', description="minimum median flow so that frame is accepted as keyframe, especially important for standstill", default="8.", min="0.", max="200.", configurable=True)
gen.add("critical_rotation_difference", paramtype='double', description="if rotation between last frame and that is bigger keyframe is selected, important for curves", default="0.08", min="0.", max="1.5", configurable=True)

gen.add("robust_loss_depth_thres", paramtype='double', description="threshold in meters for robustifier(cauchy loss) for depth diff, from this threshold on measurements are considered as outlier", default="0.15", min="0.0001", max="5.0", configurable=True)
gen.add("robust_loss_reprojection_thres", paramtype='double', description="threshold in pixels for robustifier(huber loss) for reprojection diff, from this threshold on measurements are considered as outlier", default="1.0", min="0.0001", max="10.", configurable=True)
gen.add("min_number_connecting_landmarks", paramtype='int', description="if number of tracklets that connect first to current frame is smaller than this, we set the end of the optimization window", default="30", min="5", max="500", configurable=True)
gen.add("max_size_optimization_window", paramtype='int', description="maximum number of keyframes in optimization window.", default="20", min="3", max="500", configurable=True)

gen.add("shrubbery_weight", paramtype='double', description="Weight assigned to landmarks with label that belongs to vegetation, sky, terrain, ...", default="0.9", min="0.", max="1.", configurable=True)

gen.add("outlier_rejection_quantile", paramtype='double', description="we solve the problem n times with n num_iterations and reject the (1-quantile)*100% most downweighted residuals", default="0.95", min="0.01", max="1.0", configurable=True)
gen.add("outlier_rejection_num_iterations", paramtype='int', description="we solve the problem n times with n num_iterations and reject the (1-quantile)*100% most downweighted residuals", default="1", min="0", max="10", configurable=True)

gen.add("height_over_ground", paramtype='double', description="height over ground from vehicle coordinate system, value<-10. means no gp will be added to problem.", default="0.30", min="-10.0001", max="10", configurable=True)
gen.add("prior_speed", paramtype='double', description="Speed of vehicle in meter per second at beginning, when plane can not be estimated yet. Only used if not external prior given.  ", default="4.", min="0.001", max="30.", configurable=True)

# Other parameters
gen.add("show_debug_image", paramtype='bool', description="if true show measurements on keyframes in debug image", default="false", configurable=True)
gen.add("max_solver_time", paramtype='double', description="maximum solver time in seconds, use that to make it real time", default="0.2", min="0.01", max="20.0", configurable=True)
gen.add("dump_path", paramtype='std::string', description="path to dump file", default="", configurable=True)
gen.add("outlier_labels_yaml", paramtype='std::string', description="path to file for outlier labels", default="../res/outlier_labels.yaml", configurable=True)

# DO NOT TOUCH THIS LINE
#Syntax : Package, Node, Config Name(The final name will be MonoLidarConfig)
exit(gen.generate("keyframe_bundle_adjustment_ros_tool", "mono_lidar", "MonoLidar"))
